/**
 * @file src/driver/kobuki.cpp
 *
 * @brief Implementation for the kobuki device driver.
 *
 * License: BSD
 *   https://raw.github.com/yujinrobot/kobuki_core/hydro-devel/kobuki_driver/LICENSE
 **/
/*****************************************************************************
 ** Includes
 *****************************************************************************/

//#include <stdexcept>
#include "mobile.hpp"
#include "packet_handler/payload_headers.hpp"
#include "diff_driver.hpp"

//#include "iostream"

/*****************************************************************************
 ** Namespaces
 *****************************************************************************/
using namespace std;
/*****************************************************************************
 ** Implementation [PacketFinder]
 *****************************************************************************/

bool PacketFinder::checkSum()
{
  unsigned int packet_size(buffer.size());
  unsigned char cs(0);
  for (unsigned int i = 2; i < packet_size; i++)
  {
    cs ^= buffer[i];
  }
  return cs ? false : true;
}

/*****************************************************************************
 ** Implementation [Initialisation]
 *****************************************************************************/

Mobile::Mobile() :
    shutdown_requested(false)
    , is_enabled(false)
    , is_connected(false)
    , is_alive(false)
    , version_info_reminder(0)
    , controller_info_reminder(0)
    , heading_offset(0.0/0.0)
    , velocity_commands_debug(4, 0)
    , last_signal_time(millis())
    , cmd_time(millis())
    , speed(0)
    , radius(0)
{
  // master = new Modbus(0, 2, 0);
}

/**
 * Shutdown the driver - make sure we wait for the thread to finish.
 */
Mobile::~Mobile()
{
  disable();
  shutdown_requested = true; // thread's spin() will catch this and terminate
  //thread.join();
  //sig_debug.emit("Device: kobuki driver terminated.");
}

void Mobile::init(Parameters &parameters)
{

  if (!parameters.validate())
  {
    //throw ecl::StandardException(LOC, ecl::ConfigurationError, "Mobile's parameter settings did not validate.");
  }
  this->parameters = parameters;
  std::string sigslots_namespace = parameters.sigslots_namespace;

  std::deque<unsigned char> stx;//(2, 0);
  std::deque<unsigned char> etx;//(1);
  stx.push_back(0xaa);
  stx.push_back(0x55);
  packet_finder.configure(sigslots_namespace, stx, etx, 1, 256, 1, true);
  dock_ir_packet_finder.configure();
  imu_packet_finder.configure();
  acceleration_limiter.init(parameters.enable_acceleration_limiter);

  // in case the user changed these from the defaults
  Battery::capacity = parameters.battery_capacity;
  Battery::low = parameters.battery_low;
  Battery::dangerous = parameters.battery_dangerous;

  /******************************************
   ** Get Version Info Commands
   *******************************************/
  version_info_reminder = 10;
  //sendCommand(Command::GetVersionInfo());

  /******************************************
   ** Get Controller Info Commands
   *******************************************/
  controller_info_reminder = 10;
  //sendCommand(Command::GetControllerGain());
  //sig_controller_info.emit(); //emit default gain

  //thread.start(&Mobile::spin, *this);

  // feedback_buf[0] = 0xAA;
  // feedback_buf[1] = 0x55;

  //Send By default
  feedback_vec.push_back(&core_sensors);
  feedback_vec.push_back(&dock_ir);
  feedback_vec.push_back(&inertia);
  feedback_vec.push_back(&cliff);
  feedback_vec.push_back(&current);
  feedback_vec.push_back(&three_axis_gyro);
  feedback_vec.push_back(&gp_input);

  unique_device_id.data.udid0 = 1;
  unique_device_id.data.udid1 = 4;
  unique_device_id.data.udid2 = 7;


  Serial2.begin(115200, SERIAL_8N2);
  delay(100);
  // Serial3.setTimeout(1);
  node.begin(1, Serial2);

  // master.begin(9600, SERIAL_8N2);
  // master.begin( 9600, SERIAL_8N2); // baud-rate at 19200
}

/*****************************************************************************
 ** Implementation [Runtime]
 *****************************************************************************/

/**
 * @brief Performs a scan looking for incoming data packets.
 *
 * Sits on the device waiting for incoming and then parses it, and signals
 * that an update has occured.
 *
 * Or, if in simulation, just loopsback the motor devices.
 */



void Mobile::run()
{

  unsigned long timeout = 100;

  while(Serial.available()) {
  //while(false) {
    int n = Serial.readBytes(receive_buf, packet_finder.numberOfDataToRead());
    //Serial.print("readBytes=");
    //Serial.print(n);
    // if (n <= 0) continue;

    if (n > 0 && packet_finder.update(receive_buf, n)) // this clears packet finder's buffer and transfers important bytes into it
    {
      PacketFinder::BufferType local_buffer;
      packet_finder.getBuffer(local_buffer); // get a reference to packet finder's buffer.


      packet_finder.getPayload(data_buffer);// get a reference to packet finder's buffer.

      while (data_buffer.size() > 0)
      {
        if (kobuki_command.deserialise(data_buffer)) {

          if (kobuki_command.data.command == Command::GetController) {
            //Serial.print("Command::GetController\n");
            sendCommand(controller_info);
          }
          else if (kobuki_command.data.command == Command::RequestExtra) {
            //Serial.print("Command::RequestExtra\n");
            sendCommand(unique_device_id);
          }
          else if (kobuki_command.data.command == Command::BaseControl) {
            this->speed = kobuki_command.data.speed;
            this->radius = kobuki_command.data.radius;
            this->cmd_time = millis();
            // diffDriver(kobuki_command.data.speed, kobuki_command.data.radius);
            // setMotorSpeed(RIGHT_MOTOR, vel.right);
            // master.poll();
            // setMotorSpeed(LEFT_MOTOR, vel.left);
            // master.poll();

            // node.setSlaveID(1);
            // node.writeSingleRegister(0x43, vel.left);
            // node.setSlaveID(2);
            // node.writeSingleRegister(0x43, vel.right);
          }
        }
        else
          fixPayload(data_buffer);
      }
      //std::cout << "---" << std::endl;

      is_alive = true;
    //  event_manager.update(is_connected, is_alive);
    //  last_signal_time.stamp();
      last_signal_time = millis();
    //  sig_stream_data.emit();
    //  sendBaseControlCommand(); // send the command packet to mainboard;
    //  if( version_info_reminder/*--*/ > 0 ) sendCommand(Command::GetVersionInfo());
    //  if( controller_info_reminder/*--*/ > 0 ) sendCommand(Command::GetControllerGain());
    }
    else
    {
      // watchdog
      if (is_alive && ((millis() - last_signal_time) > timeout))
      {
        is_alive = false;
        // do not call here the event manager update, as it generates a spurious offline state
      }
    }
  }
}

void Mobile::getInertia()
{
  while(Serial1.available()) {
  // Serial.println(imu_packet_finder.numberOfDataToRead());
    int n = Serial1.readBytes(imu_receive_buf, imu_packet_finder.numberOfDataToRead());

    if (n > 0 && imu_packet_finder.update(imu_receive_buf, n)) // this clears packet finder's buffer and transfers important bytes into it
    {
      ImuPacketFinderBase::BufferType local_buffer;
      imu_packet_finder.getBuffer(local_buffer); // get a reference to packet finder's buffer.
      if (local_buffer[3] == 0xB1) {
        unsigned long yaw = local_buffer[4] & 0x7f;
        yaw = yaw << 16;
        yaw |= ((unsigned long)local_buffer[5] << 8);
        yaw |= local_buffer[6];
        // Serial.println(yaw);
        long yawInt= yaw / 100;
        // Serial.println(yawInt);
        if (yawInt >= 18000)
          yawInt -= 36000;
          // Serial.println(yawInt);
        setInertiaAngle((int16_t)-yawInt);
      }
      if (local_buffer[3] == 0xB0) {
        char positive = 1;
        if (local_buffer[14] & 0x80)
          positive = -1;

        long gyroZ = local_buffer[14] & 0x7f;
        // Serial.println(local_buffer[14]);
        // Serial.println(local_buffer[15]);
        gyroZ = gyroZ << 8;
        gyroZ |= local_buffer[15];
        setInertiaAngleRate(gyroZ * positive);
        // Serial.print("---\n");
        // Serial.println(gyroZ * positive);
      }
      // setInertia(-yawInt * 100, 0);


      // unsigned char dock_ir_num = 1;
      // //right --> central --> left
      // for (std::vector<uint8_t>::iterator it = dock_ir.data.docking.begin(); it != dock_ir.data.docking.end(); ++it) {
      //   (*it) = local_buffer[dock_ir_num];
      //   dock_ir_num ++;
      // }
    }
  }
}

void Mobile::fixPayload(std::deque<unsigned char> & byteStream)
{
  if (byteStream.size() < 3 ) { /* minimum size of sub-payload is 3; header_id, length, data */
    //sig_named.emit(log("error", "packet", "too small sub-payload detected."));
    //std::cout << "too small sub-payload detected" << std::endl;
    byteStream.clear();
  } else {
    std::stringstream ostream;
    unsigned int header_id = (unsigned int)(byteStream.front());
    byteStream.pop_front();
    unsigned int length = (unsigned int)(byteStream.front());
    byteStream.pop_front();
    unsigned int remains = byteStream.size();
    unsigned int to_pop;

    ostream << "[" << header_id << "]";
    ostream << "[" << length << "]";

    ostream << "[";
    ostream << std::setfill('0') << std::uppercase;
    ostream << std::hex << std::setw(2) << header_id << " " << std::dec;
    ostream << std::hex << std::setw(2) << length << " " << std::dec;

    if (remains < length) to_pop = remains;
    else                  to_pop = length;

    for (unsigned int i = 0; i < to_pop; i++ ) {
      unsigned int byte = (unsigned int)(byteStream.front());
      byteStream.pop_front();
      ostream << std::hex << std::setw(2) << byte << " " << std::dec;
    }
    ostream << "]";

    // if (remains < length)
    //   //sig_named.emit(log("error", "packet", "malformed sub-payload detected. "  + ostream.str()));
    //   std::cout << "malformed sub-payload detected." << std::endl;
    // else
    //   std::cout << "unknown sub-payload detected. " << std::endl;
      //sig_named.emit(log("debug", "packet", "unknown sub-payload detected. " + ostream.str()));
  }
}


/*****************************************************************************
 ** Implementation [Human Friendly Accessors]
 *****************************************************************************/

/*****************************************************************************
 ** Commands
 *****************************************************************************/

void Mobile::setLed(const enum LedNumber &number, const enum LedColour &colour)
{
//  sendCommand(Command::SetLedArray(number, colour, kobuki_command.data));
}

void Mobile::setDigitalOutput(const DigitalOutput &digital_output) {
//  sendCommand(Command::SetDigitalOutput(digital_output, kobuki_command.data));
}

void Mobile::setExternalPower(const DigitalOutput &digital_output) {
//  sendCommand(Command::SetExternalPower(digital_output, kobuki_command.data));
}

//void Mobile::playSound(const enum Sounds &number)
//{
//  sendCommand(Command::PlaySound(number, kobuki_command.data));
//}

void Mobile::playSoundSequence(const enum SoundSequences &number)
{
//  sendCommand(Command::PlaySoundSequence(number, kobuki_command.data));
}

// bool Mobile::setControllerGain(const unsigned char &type, const unsigned int &p_gain,
//                                const unsigned int &i_gain, const unsigned int &d_gain)
// {
//   if ((firmware.flashed_major_version() < 2) && (firmware.flashed_minor_version() < 2)) {
//     sig_warn.emit("Robot firmware doesn't support this function, so you must upgrade it. " \
//                   "Consult how-to on: http://kobuki.yujinrobot.com/home-en/documentation/howtos/upgrading-firmware");
//     sig_warn.emit("Robot firmware version is " + VersionInfo::toString(firmware.data.version)
//                 + "; latest version is " + firmware.current_version());
//     return false;
//   }

//   sendCommand(Command::SetControllerGain(type, p_gain, i_gain, d_gain));
//   return true;
// }

// bool Mobile::getControllerGain()
// {
//   if ((firmware.flashed_major_version() < 2) && (firmware.flashed_minor_version() < 2)) {
//     sig_warn.emit("Robot firmware doesn't support this function, so you must upgrade it. " \
//                   "Consult how-to on: http://kobuki.yujinrobot.com/home-en/documentation/howtos/upgrading-firmware");
//     sig_warn.emit("Robot firmware version is " + VersionInfo::toString(firmware.data.version)
//                 + "; latest version is " + firmware.current_version());
//     return false;
//   }

//   sendCommand(Command::GetControllerGain());
//   return true;
// }

unsigned char Mobile::getNumberOfDataToRead()
{
  return packet_finder.numberOfDataToRead();
}

// void Mobile::setBaseControl(const double &linear_velocity, const double &angular_velocity)
// {
//   diff_drive.setVelocityCommands(linear_velocity, angular_velocity);
// }

// void Mobile::sendBaseControlCommand()
// {
//   std::vector<double> velocity_commands_received;
//   if( acceleration_limiter.isEnabled() ) {
//     velocity_commands_received=acceleration_limiter.limit(diff_drive.pointVelocity());
//   } else {
//     velocity_commands_received=diff_drive.pointVelocity();
//   }
//   diff_drive.velocityCommands(velocity_commands_received);
//   std::vector<short> velocity_commands = diff_drive.velocityCommands();
//   //std::cout << "speed: " << velocity_commands[0] << ", radius: " << velocity_commands[1] << std::endl;
//   sendCommand(Command::SetVelocityControl(velocity_commands[0], velocity_commands[1]));

//   //experimental; send raw control command and received command velocity
//   velocity_commands_debug=velocity_commands;
//   velocity_commands_debug.push_back((short)(velocity_commands_received[0]*1000.0));
//   velocity_commands_debug.push_back((short)(velocity_commands_received[1]*1000.0));
//   sig_raw_control_command.emit(velocity_commands_debug);
// }

/**
 * @brief Send the prepared command to the serial port.
 *
 * Need to be a bit careful here, because we have no control over how the user
 * is calling this - they may be calling from different threads (this is so for
 * kobuki_node), so we mutex protect it here rather than relying on the user
 * to do so above.
 *
 * @param command : prepared command template (see Command's static member functions).
 */
void Mobile::sendCommand(Command command)
{
  if( !is_alive || !is_connected ) {
    //need to do something
    //sig_debug.emit("Device state is not ready yet.");
    if( !is_alive     )
      //sig_debug.emit(" - Device is not alive.");
      std::cout << " - Device is not alive." << std::endl;
    if( !is_connected )
      //sig_debug.emit(" - Device is not connected.");
      std::cout << " - Device is not connected." << std::endl;
    //std::cout << is_enabled << ", " << is_alive << ", " << is_connected << std::endl;
    return;
  }
  //command_mutex.lock();
  kobuki_command.resetBuffer(command_buffer);

  if (!command.serialise(command_buffer))
  {
    std::cout << "command serialise failed." << std::endl;
    //sig_error.emit("command serialise failed.");
  }
  command_buffer[2] = command_buffer.size() - 3;
  unsigned char checksum = 0;
  for (unsigned int i = 2; i < command_buffer.size(); i++)
    checksum ^= (command_buffer[i]);

  command_buffer.push_back(checksum);
  //check_device();
  //serial.write((const char*)&command_buffer[0], command_buffer.size());

//  sig_raw_data_command.emit(command_buffer);
//  command_mutex.unlock();
}

void Mobile::sendCommand(payloadBase &payload_base)
{
  std::deque<unsigned char> dq_tmp;
  payload_base.serialise(dq_tmp);

  dq_tmp.push_front(dq_tmp.size());//add size
  dq_tmp.push_back( getCs(dq_tmp) );//add cs

  for (int i = 0; i < dq_tmp.size(); i ++)
    feedback_buf[HEADER_LEN + i] = dq_tmp[i];

  if ((dq_tmp.size() + HEADER_LEN) > MAX_FB_DATA) return;

  // Serial.print("\n********\n");
  // for (int i = 0; i < dq_tmp.size() + HEADER_LEN; i++)
  // {
  //   Serial.print(feedback_buf[i]);
  //   Serial.print("\t");
  //   /* code */
  // }
  // Serial.print("\n---------\n");

  Serial.write(feedback_buf, dq_tmp.size() + HEADER_LEN);
//  dq_tmp.clear();
}

bool Mobile::enable()
{
  is_enabled = true;
  return true;
}

bool Mobile::disable()
{
//  setBaseControl(0.0f, 0.0f);
//  sendBaseControlCommand();
  is_enabled = false;
  return true;
}

/*
以下数字来自电机驱动手册
*/
#define MOTOR_REGISTER (0x20)//address
#define MOTOR_DATA_LEN (23)  //data length
#define ENCODER_LOW    (5)
void Mobile::getEncoderFromMotor(uint8_t slaveId)
{
  uint8_t  result;
  uint16_t data[MOTOR_DATA_LEN];

  node.setSlaveID(slaveId);

  result = node.readHoldingRegisters(0x20, 0x17);
  // setEncoder(result, slaveId);
  if (result == node.ku8MBSuccess) {
    for (int j = 0; j < MOTOR_DATA_LEN; j++) {
      data[j] = node.getResponseBuffer(j);
    }
  }

  setEncoder(data[5], slaveId);
}



// void Mobile::setMotorSpeed(uint8_t motorId, int16_t speed)
// {
//   telegram.u8id = motorId; // slave address
//   telegram.u8fct = 6; // function code (this one is registers read)
//   telegram.u16RegAdd = 0x43; // start address in slave
//   telegram.u16CoilsNo = 0; // number of elements (coils or registers) to read
//   telegram.au16reg = au16dataSend; // pointer to a memory array in the Arduino
//   au16dataSend[0] = speed;

//   // Serial.println("setMotorSpeed");
//   master.command( telegram ); // send query (only once)
// }

// void Mobile::getEncoderReq(uint8_t motorId)
// {
//   telegram.u8id = motorId; // slave address
//   telegram.u8fct = 3; // function code (this one is registers read)
//   telegram.u16RegAdd = MOTOR_REGISTER; // start address in slave
//   telegram.u16CoilsNo = MOTOR_DATA_LEN; // number of elements (coils or registers) to read
//   telegram.au16reg = au16data; // pointer to a memory array in the Arduino

//   master.query( telegram ); // send query (only once)}
// }

// void Mobile::poll()
// {
//   uint8_t poll_size = master.poll(); // check incoming messages
//   // Serial.print("poll_size = ");
//   // Serial.println(poll_size);

//   if (!poll_size) return;

//   uint8_t slaveId;
//   uint16_t encoder;
//   uint8_t stat = master.getState();

//   // Serial.print("stat = ");
//   // Serial.println(stat);

//   if (stat == COM_IDLE) {
//     slaveId = master.getRxU8id();
//     encoder = au16data[ENCODER_LOW];
//     setEncoder(encoder, slaveId);
//   }
// }

void Mobile::funcFeedback()
{
  std::deque<unsigned char> send_deque, dq;
  unsigned char cs = 0;
  unsigned char size = 0;
//  send_deque.push_back(0xAA);
//  send_deque.push_back(0x55);
//  send_deque.push_back(0x00);

  for (std::vector<payloadBase *>::iterator it = feedback_vec.begin(); it != feedback_vec.end(); ++it) {

    //sendCommand(**it);
    dq.clear();
    (*it)->serialise(dq);
    send_deque.insert(send_deque.end(), dq.rbegin(), dq.rend());
  }

  size = send_deque.size();
  if (size >= 0xff) return;



  send_deque.push_front(size);

  cs = getCs(send_deque);
  send_deque.push_back(cs);

  send_deque.push_front(0x55);
  send_deque.push_front(0xAA);

  uint8_t send_size = send_deque.size();
  for (int j = 0; j < send_size; j ++) {
    feedback_buf[j] = send_deque[j];
  }

  Serial.write(feedback_buf, send_size);

  /*
  send_deque shuld be [0xAA 0x55 (payloads size) (payloads...) cs]
  */
}

void Mobile::clearCache()
{
  for (std::vector<uint8_t>::iterator it = dock_ir.data.docking.begin(); it != dock_ir.data.docking.end(); ++it) {
    (*it) = 0;
  }
}

void Mobile::setIR(const long right, const long center, const long left)
{
  dock_ir.setIR(right, 0);
  dock_ir.setIR(center, 1);
  dock_ir.setIR(left, 2);
}

// void Mobile::setEncoder(const unsigned short &left, const unsigned short &right)
// {
//   core_sensors.data.left_encoder = left;
//   core_sensors.data.right_encoder = right;
// }

void Mobile::setEncoder(const uint16_t number, const uint8_t motor)
{
  switch (motor) {
    case LEFT_MOTOR:
      core_sensors.data.left_encoder = -(number);
      break;
    case RIGHT_MOTOR:
      core_sensors.data.right_encoder = number;
      break;
    default:
      break;
  }
}

void Mobile::setInertiaAngle(const int16_t &angle)
{
  inertia.data.angle = angle;
  // inertia.data.angle_rate = angle_rate;
}

void Mobile::setInertiaAngleRate(const int16_t &angle_rate)
{
  // inertia.data.angle = angle;
  inertia.data.angle_rate = angle_rate;
}

void Mobile::setBumper(const uint8_t &left, const uint8_t &center, const uint8_t &right)
{
  core_sensors.data.bumper = 0;
  if (left) core_sensors.data.bumper |= CoreSensors::Flags::LeftBumper;
  if (center) core_sensors.data.bumper |= CoreSensors::Flags::CenterBumper;
  if (right) core_sensors.data.bumper |= CoreSensors::Flags::RightBumper;
}

void Mobile::setCliff(const int left_pin, const int center_pin, const int right_pin)
{
  int sensorValue;
  core_sensors.data.cliff = 0;
  
  sensorValue = map(analogRead(left_pin), 0, 1023, 0, 5000);
  if (sensorValue < 800)
    core_sensors.data.cliff |= CoreSensors::Flags::LeftCliff;

  sensorValue = map(analogRead(center_pin), 0, 1023, 0, 5000);
  if (sensorValue < 800)
    core_sensors.data.cliff |= CoreSensors::Flags::CenterCliff;

  sensorValue = map(analogRead(right_pin), 0, 1023, 0, 5000);
  if (sensorValue < 800)
    core_sensors.data.cliff |= CoreSensors::Flags::RightCliff;
}

void Mobile::setBattery(const uint16_t &volt)
{
  int16_t outputValue = map(volt, 0, 1023, 0, 500);
  uint8_t percent = map(outputValue, 220, 265, 0, 100);
  percent = percent > 100 ? 100 : percent;

  core_sensors.data.battery = percent;
}

void Mobile::move()
{
  DiffVel vel;

  // timeout
  if ((unsigned long)(millis() - this->cmd_time) > MOVE_TIME_OUT) {
    this->speed = 0;
    this->radius = 0;
  }
  // this->speed = 200;
  // this->radius = 200;
  /* todo
    if speed == 0 and radius == 0 and motor feedback of speed == 0
    then
      not send commands to motor drivers
  */
  vel = diffDriver(this->speed, this->radius);
  node.setSlaveID(1);
  node.writeSingleRegister(0x43, vel.left);
  node.setSlaveID(2);
  node.writeSingleRegister(0x43, vel.right);
}

//jiao yan ma
unsigned char Mobile::getCs(std::deque<unsigned char> &dq)
{
  unsigned char cs = 0;
  for (std::deque<unsigned char>::iterator it = dq.begin(); it != dq.end(); ++it) {
    cs ^= *it;
  }

  return cs;
}
